package org.opentcs.kcvehicle.communication.kc.udp.Service;

import java.util.HashMap;
import org.opentcs.drivers.vehicle.MovementCommand;
import org.opentcs.kcvehicle.communication.kc.common.byteutils.ByteUtils;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.NavigationParam;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.Path;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.Point;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;

public class HybridNavigation
    extends
      BaseCommand {

  /**
   * 测试模式：true=开启测试，false=关闭测试
   * 测试模式因orderName为String类型数据，无法转换为Integer
   */
  private static boolean TEST_MODEL = false;
  /**
   * 订单ID
   * 订单的唯一标识。用于标识多个任务KEY是否属于同一个订单。机器人单次只能接收同一订单ID的任务，直至订单结束，调度可下发新的订单ID，订单ID从1开始累加，每次+1。
   * 注：同一订单ID支持不停车更新导航。
   */
  private static Integer orderID = 0;
  /**
   * 任务ID
   * 任务的唯一标识。与订单ID绑定，从1开始，当同一订单下发新的资源时加1；订单ID发生改变，任务KEY需要重新计数。
   */
  private static Integer taskKey;
  /**
   * 序列号
   * 用于定位点在整个任务中的位置。目的是区分同一个点ID是否在一个任务中出现多次。从0开始偶数递增，例如：0->2->4->6……
   */
  private static Integer pointSerialNum = 0;
  /**
   * 用于定位段在整个任务中的位置。目的是区分同一个段ID是否在一个任务中出现多次。从1开始奇数递增，例如：1->3->5->7……
   */
  private static Integer pathSerialNum = 1;
  /**
   * 订单名映射int类型数据.
   */
  private static HashMap<String, Integer> orderNameMap = new HashMap<>();
  /**
   * 订单映射最大订单ID.
   */
  private static int currentMaxiOrderId = 50;


  /**
   * decs: 导航控制
   * 指令：0xAE
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent navigationControl(
      Integer sourcePoint, Integer destinationPoint, Integer pathID, String operation
  ) {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_MIXED_ISSUANCE_TASK);
    //TODO 构建

    Integer oldPointSerialNum = pointSerialNum;
    pointSerialNum += 2;
    Point[] points = new Point[]{
        new Point(
            oldPointSerialNum, sourcePoint, 0f, (byte) 0x00, ByteUtils.usintTo1Byte(0), null
        ),
        new Point(
            pointSerialNum, destinationPoint, 0f, (byte) 0x00, ByteUtils.usintTo1Byte(0), null
        )
    };


    Path[] paths = new Path[]{
        new Path(
            pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(0), 1f, 1f, null
        ),
    };
    pathSerialNum += 2;
    NavigationParam navigationParam = new NavigationParam(
        orderID, taskKey, ByteUtils.usintTo1Byte(points.length), ByteUtils.usintTo1Byte(
            points.length - 1
        ), points, paths
    );
    taskKey++;
    agvEvent.setBody(navigationParam.toBytes());

    return agvEvent;
  }

  /**
   *
   * @param orderName 订单ID
   * @param sourcePoint 下发起点
   * @param destinationPoint 下发终点
   * @param operation 执行操作
   */
  public static void command(
      String orderName, String sourcePoint, String destinationPoint, String operation
  ) {
//    if (TEST_MODEL) {
//      return;
//    }

    Integer newOrderName = getUniqueOrderID(orderName);
    Integer newSourcePoint = Integer.parseInt(sourcePoint);
    Integer newDestinationPoint = Integer.parseInt(destinationPoint);
    //拼接起点终点字符串转为Integer类型获取唯一pathID
    Integer pathID;
    if (newSourcePoint <= newDestinationPoint) {
      pathID = Integer.parseInt(sourcePoint + destinationPoint);
    } else {
      pathID = Integer.parseInt(destinationPoint + sourcePoint);
    }
    System.out.println("pathID:" + pathID);

    if (!orderID.equals(newOrderName)) {
      //切换订单重置参数
      initParams(newOrderName);
    }

    //0xAE(导航控制导航点控制)
    AgvEvent agvEvent = navigationControl(newSourcePoint, newDestinationPoint, pathID, operation);
    printInfo(agvEvent);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if (rcv.isOk()) {
      System.out.println("0xAE ok");
    }
    else {
      System.out.println();
      System.out.println("0xAE fail");
      System.out.println("received transationId : " + "isok:" + rcv.isOk());
    }
  }

  /**
   * 初始化参数
   *
   * @param newOrderName 新的订单ID
   */
  private static void initParams(Integer newOrderName) {
    orderID = newOrderName;
    taskKey = 1;
    pointSerialNum = 0;
    pathSerialNum = 1;
  }

  /**
   * 维护订单名对应int类型唯一ID--------todo 待优化：如果调度重启，控制器也需要重启。否则0xAE指令会因为重置订单ID和任务key下发失败。应改成wms数据库ID交互
   * @param orderName 订单名
   * @return Integer
   */
  private static Integer getUniqueOrderID(String orderName){

    Integer orderId;

    if (orderNameMap.containsKey(orderName)) {
      //订单名已存在
      orderId = orderNameMap.get(orderName);
    } else {
      //订单名不存在，创建对应映射
      currentMaxiOrderId = currentMaxiOrderId + 1;
      orderId = currentMaxiOrderId;
      orderNameMap.put(orderName, orderId);
    }

    return orderId;
  }

  /**
   * 任务结束删除唯一orderID.
   * @param command 订单
   */
  public static void delUniqueOrderID(MovementCommand command){
    String orderName = command.getTransportOrder().getName();
    orderNameMap.remove(orderName);
  }
}
